/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, 2013, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         David V. Lu!!
*********************************************************************/
#include <algorithm>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <boost/thread.hpp>
#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace costmap_2d
{

InflationLayer::InflationLayer()
        : resolution_(0), inflation_radius_(0), inscribed_radius_(0), weight_(0), inflate_unknown_(false),
          cell_inflation_radius_(0), cached_cell_inflation_radius_(0), dsrv_(NULL), seen_(NULL),
          cached_costs_(NULL), cached_distances_(NULL), last_min_x_(-std::numeric_limits<float>::max()),
          last_min_y_(-std::numeric_limits<float>::max()), last_max_x_(std::numeric_limits<float>::max()),
          last_max_y_(std::numeric_limits<float>::max())
{
    inflation_access_ = new boost::recursive_mutex();
}

void InflationLayer::onInitialize()
{
    {
        boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
        ros::NodeHandle nh("~/" + name_), g_nh;
        current_ = true;
        delete[] seen_;
        seen_ = NULL;
        seen_size_ = 0;
        need_reinflation_ = false;
        dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb =
                [this](auto &config, auto level) { reconfigureCB(config, level); };
        //dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
        //    &InflationLayer::reconfigureCB, this, _1, _2);
        if (dsrv_ != NULL)
        {
            dsrv_->clearCallback();
            dsrv_->setCallback(cb);
        }
        else
        {
            dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
            dsrv_->setCallback(cb);
        }
    }
    matchSize();
}

void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
{
    setInflationParameters(config.inflation_radius, config.cost_scaling_factor);
    if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown)
    {
        enabled_ = config.enabled;
        inflate_unknown_ = config.inflate_unknown;
        need_reinflation_ = true;
    }
}

void InflationLayer::matchSize()
{
    boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
    //Step 1:从layered_costmap_对象调用getCostmap获得costmap来实例化costmap
    costmap_2d::Costmap2D *costmap = layered_costmap_->getCostmap();
    //Step 2:获取这个costmap的分辨率
    resolution_ = costmap->getResolution();
    //Step 3:获得栅格膨胀半径
    cell_inflation_radius_ = cellDistance(inflation_radius_);
    //Step 4:计算缓存
    computeCaches();
    unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
    delete[] seen_;
    seen_size_ = size_x * size_y;
    seen_ = new bool[seen_size_];
}

// need_reinflation_默认false，更新bound这里和其他两层的主要区别是，膨胀层在传入的bound的值的基础上，通过inflation_radius_再次扩张
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x,
                                  double *min_y, double *max_x, double *max_y)
{
    if (need_reinflation_)
    {
        last_min_x_ = *min_x;
        last_min_y_ = *min_y;
        last_max_x_ = *max_x;
        last_max_y_ = *max_y;
        // For some reason when I make these - <double>::max() it does not work with Costmap2D::worldToMapEnforceBounds(),
        // so I'm using - <float>::max() instead.
        *min_x = -std::numeric_limits<float>::max();
        *min_y = -std::numeric_limits<float>::max();
        *max_x = std::numeric_limits<float>::max();
        *max_y = std::numeric_limits<float>::max();
        need_reinflation_ = false;
    }
    else
    {
        double tmp_min_x = last_min_x_;
        double tmp_min_y = last_min_y_;
        double tmp_max_x = last_max_x_;
        double tmp_max_y = last_max_y_;
        last_min_x_ = *min_x;
        last_min_y_ = *min_y;
        last_max_x_ = *max_x;
        last_max_y_ = *max_y;
        *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
        *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
        *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
        *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
    }
}

void InflationLayer::onFootprintChanged()
{
    inscribed_radius_ = layered_costmap_->getInscribedRadius(); // 机器人内切圆的半径
    cell_inflation_radius_ = cellDistance(inflation_radius_);   // 膨胀栅格的半径
    computeCaches(); // 计算缓存
    need_reinflation_ = true;
    ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
              " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
              layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}

// 用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置
void InflationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
    boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
    if (cell_inflation_radius_ == 0) return;
    // make sure the inflation list is empty at the beginning of the cycle (should always be true)
    // Step 1:首先确保在周期开始时膨胀列表为空（应始终为真）
    ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
    // 跟master_grid有关 获取地图
    unsigned char *master_array = master_grid.getCharMap();
    unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
    // 使用seen_来做标记
    if (seen_ == NULL)
    {
        ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
        seen_size_ = size_x * size_y;
        seen_ = new bool[seen_size_];
    }
    else if (seen_size_ != size_x * size_y)
    {
        ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
        delete[] seen_;
        seen_size_ = size_x * size_y;
        seen_ = new bool[seen_size_];
    }
#ifdef SEMANTIC
    if (!layered_costmap_->isRolling())
    {
        min_i -= cell_inflation_radius_;
        min_j -= cell_inflation_radius_;
        max_i += cell_inflation_radius_;
        max_j += cell_inflation_radius_;
        min_i = std::max(0, min_i);
        min_j = std::max(0, min_j);
        max_i = std::min(int(size_x), max_i);
        max_j = std::min(int(size_y), max_j);
    }
    // 接下来遍历bound中的cell，找到cost为LETHAL_OBSTACLE，即障碍物cell，将其以CellData形式放进inflation_cells_[0.0]中,
    // 自然距离为0对应的cell即障碍物cell本身，目前得到的inflation_cells_只包括障碍物本身。
    // Step 2:以距离障碍物的距离为标准给bound内的cell归类，为膨胀做准备.
    std::vector<CellData> &obs_bin = inflation_cells_[0.0];
    unsigned int index;
    // 从致命障碍物(lethal obstacles)开始，根据定义，距离为0.0
    for (int j = min_j; j < max_j; j++)
    {
        for (int i = min_i; i < max_i; i++)
        {
            if (inflateCell(master_grid, index, i, j))
            {
                obs_bin.emplace_back(index, i, j, i, j, toXJUoption(master_array[index]));
            }
            seen_[index] = false;
        }
    }
    // Step 3:通过增加距离来处理单元；新单元格被附加到相应的距离条柱中，因此它们可以超越先前插入但距离较远的单元格。
    // a.外层遍历“键”，即距障碍物的距离；内层遍历“值”，即cell。
    for (auto & inflation_cell : inflation_cells_)
    {
        // 1)将std::vector<CellData> inflation_cell.second里的元素取出来，赋值给cell
        for (const auto & cell : inflation_cell.second)
        {
            // process all cells at distance dist_bin.first 远距离处理所有单元格
            // 2)取出cell的索引index
            unsigned int index = cell.index_;
            // 3)ignore if already visited 如果已访问过，则忽略
            if (seen_[index]) continue;
            auto option = cell.option_;
            // only set current max to seen_
            if (toXJUoption(master_array[index]) == option ||
                (toXJUoption(master_array[index]) == XJU_OPTION_NORM && option == XJU_OPTION_INIT))
            {
                seen_[index] = true;
            }
            unsigned int mx = cell.x_;
            unsigned int my = cell.y_;
            unsigned int sx = cell.src_x_;
            unsigned int sy = cell.src_y_;
            // assign the cost associated with the distance from an obstacle to the cell
            // 4)分配与障碍物到单元格的距离相关联的代价
            unsigned char cost = costLookup(mx, my, sx, sy);
            // 旧celcost，是master地图上的cost
            unsigned char old_cost = master_array[index];
            // 5)master_array来自master_grid，这里再次刷新
            // inflate_unknown_为true：当新cost>0，设为新cost；
            // inflate_unknown_为false：新cost≥INSCRIBED_INFLATED_OBSTACLE，设为新cost
            // 区别就在于，如果inflate unknown，则当膨胀到主地图上的未知区域，只要有cost，就覆盖它；
            // 而当inflate unknown关闭，当膨胀到主地图上的未知区域时，只有新cost是障碍物，才覆盖它，否则维持未知。后者膨胀的范围更窄！
            if (old_cost == XJU_COST_NO_INFORMATION &&
                (inflate_unknown_ ? (cost > XJU_COST_FREE_SPACE) : (cost >= XJU_COST_INSCRIBED_INFLATED_OBSTACLE)))
                setXJUcost(master_array[index], cost);
            else setXJUcost(master_array[index], std::max(old_cost, cost));
            // attempt to put the neighbors of the current cell onto the inflation list
            // 6)把当前cell的四周的点按照距离远近加入inflation_cells_对应的键下
            if (mx > min_i) enqueue(master_grid, index - 1, mx - 1, my, sx, sy, option);
            if (my > min_j) enqueue(master_grid, index - size_x, mx, my - 1, sx, sy, option);
            if (mx < max_i - 1) enqueue(master_grid, index + 1, mx + 1, my, sx, sy, option);
            if (my < max_j - 1) enqueue(master_grid, index + size_x, mx, my + 1, sx, sy, option);
        }
    }
    for (auto & cell : inflation_cells_[0.0]) setXJUoption(master_array[cell.index_], cell.option_);
#else
    memset(seen_, false, size_x * size_y * sizeof(bool));
    // We need to include in the inflation cells outside the bounding box min_i...max_j, by the amount cell_inflation_radius_.
    // Cells up to that distance outside the box can still influence the costs stored in cells inside the box.
    // 我们需要在边界框外的膨胀单元格中包括min_i...max_j，按cell_inflation_radius_量。
    // 在box外达到该距离的单元格仍会影响box内的单元格存储的成本。
    // 边缘膨胀
    min_i -= cell_inflation_radius_;
    min_j -= cell_inflation_radius_;
    max_i += cell_inflation_radius_;
    max_j += cell_inflation_radius_;
    min_i = std::max(0, min_i);
    min_j = std::max(0, min_j);
    max_i = std::min(int(size_x), max_i);
    max_j = std::min(int(size_y), max_j);
    // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
    // We use a map<distance, list> to emulate the priority queue used before,
    // with a notable performance boost Start with lethal obstacles: by definition distance is 0.0
    // 接下来遍历bound中的cell，找到cost为LETHAL_OBSTACLE，即障碍物cell，将其以CellData形式放进inflation_cells_[0.0]中,
    // 自然距离为0对应的cell即障碍物cell本身，目前得到的inflation_cells_只包括障碍物本身。
    // Step 2:以距离障碍物的距离为标准给bound内的cell归类，为膨胀做准备.
    std::vector<CellData> &obs_bin = inflation_cells_[0.0];
    // 从致命障碍物(lethal obstacles)开始，根据定义，距离为0.0
    for (int j = min_j; j < max_j; j++)
    {
        for (int i = min_i; i < max_i; i++)
        {
            // 1)从master_grid地图获取(i,j)对应的索引index
            int index = master_grid.getIndex(i, j);
            // 2)从master_grid地图获取索引对应的代价cost
            unsigned char cost = master_array[index];
            // 3)先判断代价是否等于致命障碍物对应的代价，如果是，加入obs_bin
            if (cost == LETHAL_OBSTACLE) obs_bin.emplace_back(index, i, j, i, j);
        }
    }
    // Process cells by increasing distance; new cells are appended to the corresponding distance bin,
    // so they can overtake previously inserted but farther away cells
    // Step 3:通过增加距离来处理单元；新单元格被附加到相应的距离条柱中，因此它们可以超越先前插入但距离较远的单元格。
    // a.外层遍历“键”，即距障碍物的距离；内层遍历“值”，即cell。
    // std::map<double, std::vector<CellData>> inflation_cells_
    for (auto & inflation_cell : inflation_cells_)
    {
        // 1)将std::vector<CellData> inflation_cell.second里的元素取出来，赋值给cell
        for (const auto & cell : inflation_cell.second)
        {
            // process all cells at distance dist_bin.first 远距离处理所有单元格
            // 2)取出cell的索引index
            unsigned int index = cell.index_;
            // 3)ignore if already visited 如果已访问过，则忽略
            if (seen_[index]) continue;
            seen_[index] = true;
            unsigned int mx = cell.x_;
            unsigned int my = cell.y_;
            unsigned int sx = cell.src_x_;
            unsigned int sy = cell.src_y_;
            // assign the cost associated with the distance from an obstacle to the cell
            // 4)分配与障碍物到单元格的距离相关联的代价
            unsigned char cost = costLookup(mx, my, sx, sy);
            // 旧celcost，是master地图上的cost
            unsigned char old_cost = master_array[index];
            // 5)master_array来自master_grid，这里再次刷新
            // inflate_unknown_为true：当新cost>0，设为新cost；
            // inflate_unknown_为false：新cost≥INSCRIBED_INFLATED_OBSTACLE，设为新cost
            // 区别就在于，如果inflate unknown，则当膨胀到主地图上的未知区域，只要有cost，就覆盖它；
            // 而当inflate unknown关闭，当膨胀到主地图上的未知区域时，只有新cost是障碍物，才覆盖它，否则维持未知。后者膨胀的范围更窄！
            if (old_cost == NO_INFORMATION &&
                (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
            {
                master_array[index] = cost;
            }
            // master_array来自master_grid,这里再次刷新
            else master_array[index] = std::max(old_cost, cost);
            // attempt to put the neighbors of the current cell onto the inflation list
            // 6)把当前cell的四周的点按照距离远近加入inflation_cells_对应的键下
            if (mx > 0) enqueue(index - 1, mx - 1, my, sx, sy);
            if (my > 0) enqueue(index - size_x, mx, my - 1, sx, sy);
            if (mx < size_x - 1) enqueue(index + 1, mx + 1, my, sx, sy);
            if (my < size_y - 1) enqueue(index + size_x, mx, my + 1, sx, sy);
        }
    }
#endif
    // Step 4:清除inflation_cells
    inflation_cells_.clear();
}

/** 给定代价地图中单元格的索引，将其放入等待障碍物膨胀的列表中
* @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
* @param  grid The costmap
* @param  index The index of the cell
* @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
* @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
* @param  src_x The x index of the obstacle point inflation started at
* @param  src_y The y index of the obstacle point inflation started at
*/
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y)
{
    if (!seen_[index])
    {
        // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
        // Step 1:找到当前cell与最近的障碍物cell的距离
        double distance = distanceLookup(mx, my, src_x, src_y);
        // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
        // Step 2:我们只想将单元格放在列表中，如果它位于障碍点的膨胀半径内.如果距离大于栅格单元的膨胀半径:
        if (distance > cell_inflation_radius_) return;
        // push the cell data onto the inflation list and mark
        // Step 3:将单元格数据推送到膨胀列表并标记
        inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
    }
}

#ifdef SEMANTIC
/**
 * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
 * @param  grid The cost & semantic map
 * @param  index The index of the cell
 * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  src_x The x index of the obstacle point inflation started at
 * @param  src_y The y index of the obstacle point inflation started at
 * @param  option The option value of obstacle point started at
 */
inline void InflationLayer::enqueue(costmap_2d::Costmap2D &grid, unsigned int index,
        unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y, uint8_t option)
{
    // we only want to put the cell valuable in to queue
    auto grid_value = toXJUgrid(grid.getCost(index));
    if (grid_value.option >= option && (seen_[index] || grid_value.cost == XJU_COST_LETHAL_OBSTACLE)) return;
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    double distance = distanceLookup(mx, my, src_x, src_y);
    // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
    if (distance > cell_inflation_radius_) return;
    inflation_cells_[distance].emplace_back(index, mx, my, src_x, src_y, option);
    setXJUoption(grid.getCharMap()[index], std::max(grid_value.option, option));
    seen_[index] = false;
}

inline bool InflationLayer::inflateCell(costmap_2d::Costmap2D const &master_grid, unsigned int &index,
                                        unsigned int i, unsigned int j) const
{
    index = master_grid.getIndex(i, j);
    auto grid_value = toXJUgrid(master_grid.getCost(index));
    return grid_value.cost == XJU_COST_LETHAL_OBSTACLE && grid_value.option != XJU_OPTION_NONE;
}

#endif

void InflationLayer::computeCaches()
{
    // cell_inflation_radius_为零说明不用膨胀了
    // Case 1:如果栅格的膨胀半径是0，那就直接返回，啥也不用干了。
    if (cell_inflation_radius_ == 0) return;
    // based on the inflation radius... compute distance and cost caches
    // Case 2:如果栅格的膨胀半径不等于缓存的栅格膨胀半径，那么: 基于膨胀半径...计算距离和成本缓存
    if (cell_inflation_radius_ != cached_cell_inflation_radius_)
    {
        deleteKernels(); // 删除kernels
        // cached_distances_和cached_costs_的行数都是cell_inflation_radius_+2
        cached_costs_ = new unsigned char *[cell_inflation_radius_ + 2];
        // 重新分配内存呢，建立cached_distances_
        cached_distances_ = new double *[cell_inflation_radius_ + 2];
        // 建立两层循环，从0到栅格膨胀半径+1，逐步填充代价和距离
        for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
        {
            // cached_distances_和cached_costs_的列数也是cell_inflation_radius_+2
            // 分配每个细颗粒的内存
            cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
            // 分配每个细颗粒的内存
            cached_distances_[i] = new double[cell_inflation_radius_ + 2];
            for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
            {
                // 每个元素的值为它到(0,0)点的距离
                cached_distances_[i][j] = hypot(i, j);
            }
        }
        // 设置cached_cell_inflation_radius_，这个if内程序不会再次进入 更新缓存的膨胀半径
        cached_cell_inflation_radius_ = cell_inflation_radius_;
    }
    // 计算代价
    for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
    {
        for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
        {
            cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
        }
    }
}

void InflationLayer::deleteKernels()
{
    // 首先判断缓存的距离是否为NULL，建立循环，细颗粒删除cached_distances_[i],结束循环后，删除大的cached_distances_, 最后将cached_distances_=NULL;
    if (cached_distances_ != NULL)
    {
        for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
        {
            if (cached_distances_[i]) delete[] cached_distances_[i];
        }
        delete[] cached_distances_;
        cached_distances_ = NULL;
    }
    // 然后判断缓存的代价是否为NULL，建立缓存，细颗粒删除cached_costs_[i],结束循环后，删除大的cached_costs_,最后将cached_costs_=NULL;
    if (cached_costs_ != NULL)
    {
        for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
        {
            if (cached_costs_[i]) delete[] cached_costs_[i];
        }
        delete[] cached_costs_;
        cached_costs_ = NULL;
    }
}

void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
{
    if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
    {
        // Lock here so that reconfiguring the inflation radius doesn't cause segfaults when accessing the cached arrays
        // 在weight_不等于cost_scaling_factor,inflation_radius_不等于inflation_radius的情况下:
        // 锁定在此处，以便重新配置膨胀半径不会在访问缓存数组时导致隔离错误
        boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
        // 设置其它参数:inflation_radius_,cell_inflation_radius_,weight_;
        // 特别地将need_inflation_置为true;
        inflation_radius_ = inflation_radius;
        cell_inflation_radius_ = cellDistance(inflation_radius_);
        weight_ = cost_scaling_factor;
        need_reinflation_ = true;
        computeCaches();
    }
}

}  // namespace costmap_2d
